//--- Projeto Domus
// interactionsource.com
// Koji Pereira, Andre Veloso e Marcos Paulo

/*
 Projeto Domus
 interactionsource.com
 Koji Pereira, Andre Veloso e Marcos Paulo

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 3 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/


import processing.opengl.*;
import processing.serial.*;
import oscP5.*;
import netP5.*;
import cc.arduino.*;

Arduino arduino;
int sensorPin = 0;
int sensorPin1 = 1;
int sensor1;
int sensor1ligado = 0;
int sensor2;
int sensor2ligado = 0;

int sensorPin2 = 2;
int sensorPin3 = 3;
int sensor3;
int sensor4;
int sensor3ligado = 0;
int sensor4ligado =  0;

OscP5 oscP5;

float xrot = 0;
float zrot = 0;

float xrot_targ = 0;
float zrot_targ = 0;
float orientation = 0;

float valX;
float valY;
float valZ;

float dampSpeed = 5;

float resultante;

float oldResultante;

float diffResultante;

float varResultante = 0;

float oldValX = 0;
float oldValY = 0;
float oldValZ = 0;

float accelX = 0;
float accelY = 0;
float accelZ = 0;

float accelVector = 0;

float diffX = 0;
float diffY = 0;
float diffZ = 0;

float pitch, roll = 0;

PFont fontA;

NetAddress myRemoteLocation;

OscMessage myMessage;

int gravityTresh = 28;

void setup() {
  size(640,480, P3D);
 // smooth();
  oscP5 = new OscP5(this, 6001);

   fontA = loadFont("SansSerif-30.vlw");
     textFont(fontA, 20);
     
       /* myRemoteLocation is a NetAddress. a NetAddress takes 2 parameters,
   * an ip address and a port number. myRemoteLocation is used as parameter in
   * oscP5.send() when sending osc packets to another computer, device, 
   * application. usage see below. for testing purposes the listening port
   * and the port of the remote location address are the same, hence you will
   * send messages back to this sketch.
   */
  myRemoteLocation = new NetAddress("127.0.0.1", 5000);
  
  arduino = new Arduino(this, Arduino.list()[1], 57600);
  arduino.pinMode(sensorPin, Arduino.INPUT);
  arduino.pinMode(sensorPin1, Arduino.INPUT);
  arduino.pinMode(sensorPin2, Arduino.INPUT);
  arduino.pinMode(sensorPin3, Arduino.INPUT);

}

//void enviaOSC(str mensagem, int valor) {

     /* in the following different ways of creating osc messages are shown by example */
 //     myMessage = new OscMessage(mensagem);
 //     myMessage.add(valor); // add an int to the osc message 
  //    oscP5.send(myMessage, myRemoteLocation);
  //   print(mensage)

//}

void draw() {

 background(255);

   //Arduino
   sensor1 = arduino.analogRead(sensorPin);
   sensor2 = arduino.analogRead(sensorPin1);
   sensor3 = arduino.analogRead(sensorPin2);
   sensor4 = arduino.analogRead(sensorPin3);
   
//println("1-" + sensor1);
//println("2-" + sensor2);
//println("3-" + sensor3);
//println("4-" + sensor4);

      
    if(sensor1 > 905) {
      if (sensor1ligado == 0){
     // enviaOSC("/play1", sensor1ligado)
      sensor1ligado = 1;
     /* in the following different ways of creating osc messages are shown by example */
      myMessage = new OscMessage("/play1");
      myMessage.add(1); // add an int to the osc message 
      oscP5.send(myMessage, myRemoteLocation);
      
      //println("Play sound 1");
      
     // delay(500);
    }
    } else {
     if (sensor1ligado == 1){
     // enviaOSC("/play1", sensorligado)
     /* in the following different ways of creating osc messages are shown by example */
      myMessage = new OscMessage("/play1");
      myMessage.add(0); // add an int to the osc message 
      oscP5.send(myMessage, myRemoteLocation);
      //println("Play sound 1");
     // delay(500);
     sensor1ligado = 0;
    }
    }
   if(sensor2 > 935) {

     if (sensor2ligado == 0){
      /* in the following different ways of creating osc messages are shown by example */
      myMessage = new OscMessage("/play2");
      myMessage.add(1); // add an int to the osc message 
      oscP5.send(myMessage, myRemoteLocation);
      sensor2ligado = 1;
      //println("Play sound 2");
      
     // delay(500);
     }
    } else{
    
      if (sensor2ligado == 1){
      /* in the following different ways of creating osc messages are shown by example */
      myMessage = new OscMessage("/play2");
      myMessage.add(0); // add an int to the osc message 
      oscP5.send(myMessage, myRemoteLocation);
      sensor2ligado = 0;
      //println("Play sound 2");
      
     // delay(500);
      }
    }
    
    if(sensor3 > 970) {
      if (sensor3ligado == 0){

     /* in the following different ways of creating osc messages are shown by example */
      myMessage = new OscMessage("/play3");
      myMessage.add(1); // add an int to the osc message 
      oscP5.send(myMessage, myRemoteLocation);
     sensor3ligado = 1;
     // println("Play sound 3");
      
     // delay(500);
      }
    } else {
    if (sensor3ligado == 1){
     /* in the following different ways of creating osc messages are shown by example */
      myMessage = new OscMessage("/play3");
      myMessage.add(0); // add an int to the osc message 
      oscP5.send(myMessage, myRemoteLocation);
      sensor3ligado = 0;
     // println("Play sound 3");
      
     // delay(500);
    }
    }
   
    if(sensor4 > 945) {
if (sensor4ligado == 0){
     /* in the following different ways of creating osc messages are shown by example */
      myMessage = new OscMessage("/play4");
      myMessage.add(1); // add an int to the osc message 
      oscP5.send(myMessage, myRemoteLocation);
      sensor4ligado = 1;
}
    } else {
      if (sensor4ligado == 1){
     /* in the following different ways of creating osc messages are shown by example */
      myMessage = new OscMessage("/play4");
      myMessage.add(0); // add an int to the osc message 
      oscP5.send(myMessage, myRemoteLocation);
      sensor4ligado=0;
    }
    }
   
   

   oldResultante = resultante;

   diffResultante = resultante - 35;



    oldValX = valX;
    oldValY = valY;
    oldValZ = valZ;

   fill(0);
    text("Forca Res:", 30, 30);
   text(resultante/34, 150, 30);
    text("G:", 220, 30);

    text("VAr Res:", 30, 60);
   text(varResultante/34 , 150, 60);
   text("G:", 220, 60);



    text(accelVector, 500, 30);
    text("Accel Vector Res:", 330, 30);

  stroke(0);
  strokeWeight(50);
  line(30, 300, 30, 301-varResultante );


 // Detection for if the iPhone is upsidedown or not

  pushMatrix();
  translate(width/2, height/2, 1);
  


  
  rotateX(-PI/6*(mouseX*0.01));
  rotateY(-PI/6*(mouseY*0.01));
  
  
    fill(0);
  text("+Z:", 0, -100);
  text("-Z:", 0, 100);
  
  fill(0);
  text("-Y:", -100, 0);
  text("+Y:", 100, 0);
  
    fill(0);
  text("-X:", 0, 0, -100);
  text("+X:", 0, 0, 100);
  
  stroke(255,255,0);
  strokeWeight(2);
  line(-600,0,0, 600, 0,0);
  
  stroke(0,255,255);
  strokeWeight(2);
  line(0,-600,0, 0, 600,0);
  
    stroke(255,0,255);
  strokeWeight(2);
  line(0,0,-600, 0, 0, 600);
  
  


  //print("diffResultante: " + diffResultante);

  fill(127);
  noFill();

  if ( (diffResultante > 60) &&  (accelVector>80)  ) {
    //println(" - ######## subindo  ");
    //fill(255,0,0);
    stroke(255,0,0);
  } else if (( diffResultante < 60 ) && ( diffResultante >  -30) ) {
    //println(" - ### lento ");
     //fill(127);
   stroke(127);
  } else if ( (diffResultante < -20) ) {
    //println(" - ############## caindo ");
    //fill(0,0,255);
    stroke(0,0,255);
  }

  strokeWeight(2);
  box(70,10,40);


  //Vetor resultante
  stroke(0);
  strokeWeight(10);
  line(0,0,0, valY*1.5, valZ*1.5, valX*1.5);





  //-------------------------------------
  /*
  //Vetor forca individual
  stroke(255,255,0);
  strokeWeight(5);
  line(0,0,0, -valY*1.5, 0,0);


  //Vetor forca individual
  stroke(0,255,255);
  strokeWeight(5);
  line(0,0,0, 0, -valZ*1.5,0);



  //Vetor forca individual
  stroke(255,0,255);
  strokeWeight(5);
  line(0,0,0, 0, 0, -valX*1.5);
  */

   //------------------------------
  /*
  //Vetor forca individual
  stroke(255,255,0);
  strokeWeight(5);
  line(0,0,0, -accelY*1.5, 0,0);


  //Vetor forca individual
  stroke(0,255,255);
  strokeWeight(5);
  line(0,0,0, 0, -accelZ*1.5,0);


  //Vetor forca individual
  stroke(255,0,255);
  strokeWeight(5);
  line(0,0,0, 0, 0, -accelX*1.5);
  */

  //----------------------------------------
  //Vetor accel resultante
  stroke(255,0,0);
  strokeWeight(10);
  line(0,0,0, accelY*1.5, accelZ*1.5, accelX*1.5);


  //translate(-width/2, -height/2, -1);


  popMatrix();

  //Pitch - Rotation around the lateral (X) axis
  pitch = atan2(valY, sqrt( sqrt(valX) + sqrt(valZ) )) * -1;

  //Roll  - Rotation around the longitudinal (Y) axis
  roll = atan2(valX, sqrt( sqrt(valY) + sqrt(valZ) )) * -1;


  fill(0);
  text("accelX:", 30, 400);
  text(accelX, 150, 400);

  text("accelY:", 30, 425);
  text(accelY, 150, 425);

  text("accelZ:", 30, 450);
  text(accelZ, 150, 450);

  /*
  text("Pitch:", 320, 400);
  text(pitch, 400, 400);

  text("Roll:", 320, 450);
  text(roll, 400, 450);
  */


}

void oscEvent(OscMessage theOscMessage) {




    String addr = theOscMessage.addrPattern();
    //println(addr);   // uncomment for seeing the raw message
    //float  val  = theOscMessage.get(0).floatValue();




   if(addr.indexOf("/wiimotePD/gX") !=-1){ // one of the faders
       valX  = Float.valueOf( theOscMessage.get(0).intValue() );
       //valX /= 100;


    }

    if(addr.indexOf("/wiimotePD/gY") !=-1){ // one of the faders
       valY = Float.valueOf( theOscMessage.get(0).intValue() );
       //valY /= 100;

    }

   if(addr.indexOf("/wiimotePD/gZ") !=-1){ // one of the faders
       valZ  = Float.valueOf( theOscMessage.get(0).intValue() );
        //valZ  /= 100;
    }


   if ( (valX>gravityTresh) || (valX<-gravityTresh) ) {
     
     println("valor X" + valX);
     
       /* in the following different ways of creating osc messages are shown by example */
      myMessage = new OscMessage("/1/push1");
      myMessage.add(0); // add an int to the osc message 
      oscP5.send(myMessage, myRemoteLocation); 
      
      
      myMessage = new OscMessage("/1/push2");
      myMessage.add(1); // add an int to the osc message
      oscP5.send(myMessage, myRemoteLocation); 
      
      myMessage = new OscMessage("/1/push3");
      myMessage.add(1); //add an int to the osc message
      oscP5.send(myMessage, myRemoteLocation); 
      
      
     
   } else if ( (valY>gravityTresh) || (valY<-gravityTresh)  ) {
     
      println("valor Y" + valY);
     
      myMessage = new OscMessage("/1/push2");
      myMessage.add(0); /* add an int to the osc message */
      oscP5.send(myMessage, myRemoteLocation); 
      
      myMessage = new OscMessage("/1/push1");
      myMessage.add(1); // add an int to the osc message 
      oscP5.send(myMessage, myRemoteLocation); 
      
      myMessage = new OscMessage("/1/push3");
      myMessage.add(1); // add an int to the osc message 
      oscP5.send(myMessage, myRemoteLocation); 

     
   } else if ( (valZ>gravityTresh) || (valZ<-gravityTresh)  ) {
     
      println("valor z"+ valZ);
     
      myMessage = new OscMessage("/1/push3");
      myMessage.add(0); /* add an int to the osc message */
      oscP5.send(myMessage, myRemoteLocation); 
      
      
      myMessage = new OscMessage("/1/push2");
      myMessage.add(1); // add an int to the osc message
      oscP5.send(myMessage, myRemoteLocation); 
     
     
      myMessage = new OscMessage("/1/push1");
      myMessage.add(1); // add an int to the osc message
      oscP5.send(myMessage, myRemoteLocation); 

   } 


       resultante = sqrt( sq(valX) + sq(valY) + sq(valZ) );

       varResultante = (resultante-oldResultante) ;

       if (varResultante<0)
         varResultante *= -1;

       accelVector = sqrt( sq(accelX) + sq(accelY) + sq(accelZ) );


         accelX = valX - oldValX;
         accelY = valY - oldValY;
         accelZ = valZ - oldValZ;


}

